﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Space
{
    public class NewtonShip : Ship
    {
        const float Speed_TurnSpeed = 10f;
        public NewtonShip(Game game)
            : base(game)
        {
        }
    
        public float Acceleration { get; set; }
        public float TargetSpeed { get; set; }

        public float CurrentSpeed { get; set; }

        public float TurnSpeed { get; set; }

        public float TargetAngle { get; set; }

        public bool Hyperdrive { get; set; }

        public float AccelerateTo(float s, float time)
        {
            if (CurrentSpeed == s) return 0;
            float si = CurrentSpeed;
            if (s > TargetSpeed) s = TargetSpeed;
            if (Math.Abs(s - CurrentSpeed) <= Acceleration * time) CurrentSpeed = s;
            else
            {
                float accl = s > CurrentSpeed ? Acceleration : -Acceleration;
                CurrentSpeed = (CurrentSpeed + accl * time);
            }
            float speed = CurrentSpeed;
            return speed - si;
        }

        public float opt_speed(float distance)
        {
            return Math.Min((float)Math.Sqrt(distance * Acceleration), Speed);
        }

        public void SetDestination(Vector3 position)
        {
            float a = (float)Math.Atan2(position.Z - Position.Z, position.X - Position.X);
            TargetAngle = a;
            TargetSpeed = opt_speed((position - Position).Length());
        }

        public int break_distance()
        {
            return -(int)Math.Round(0.5 * Speed * Speed / Acceleration);
        }

        public void TurnTo(float angle, float time)
        {
            while (angle >= 2 * MathHelper.Pi) angle -= 2.0f * MathHelper.Pi;
            while (angle < 0) angle += 2.0f * MathHelper.Pi;

            float max_turn_speed = Math.Min(TurnSpeed, Speed_TurnSpeed / CurrentSpeed) * CurrentSpeed * time;

            if (max_turn_speed == 0) max_turn_speed = FUZZ;

            if (Math.Abs(Angle - angle) < max_turn_speed || Math.Abs(Angle - angle) > Math.PI * 2 - max_turn_speed) Angle = angle;
            else
            {
                if (Angle > angle)
                {
                    Angle += (Angle - angle) > MathHelper.Pi ? max_turn_speed : -max_turn_speed;
                }
                else
                {
                    Angle += (angle - Angle) > MathHelper.Pi ? -max_turn_speed : max_turn_speed;
                }
            }
        }

        public override void Update(float time)
        {
            AccelerateTo(TargetSpeed, time);
            TurnTo(TargetAngle, time);

            float advanceAngle = Angle;

            if(Destination!=null) {
                float arrivalRadius = Speed * 1.0f;
                float dist = (Position - Destination.Position).Length();
                if (dist <= arrivalRadius && CurrentSpeed <= dist/CurrentSpeed*Acceleration + Destination.Radius)
                {
                    advanceAngle = (float)Math.Atan2(Destination.Position.Z - Position.Z, Destination.Position.X - Position.X);

                    Arrived();
                }
            }

            Position += new Vector3((float)Math.Cos(advanceAngle), 0, (float)Math.Sin(advanceAngle)) * CurrentSpeed * (Hyperdrive ? HyperdriveMultiplier : 1) * time;

            base.Update(time);
        }

        protected virtual void Arrived() { }
    }
}
